// 
#include <iostream>
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include "preprocess.h"
#include <limits>

std::string inbagFilePath, outbagFilePath;

std::shared_ptr<Preprocess> p_pre(new Preprocess());


int main(int argc, char** argv) {
    // 初始化ROS节点
    ros::init(argc, argv, "rosbag_handler_node");
    ros::NodeHandle nh;


    nh.param<std::string>("bag/input_bag_path", inbagFilePath, "");
    nh.param<std::string>("bag/output_bag_path", outbagFilePath, "");

    // 打开rosbag文件
    rosbag::Bag inputBag;
    inputBag.open(inbagFilePath, rosbag::bagmode::Read);

    // 创建一个视图以读取整个rosbag
    rosbag::View bagView(inputBag);
    int lidarCount = 0, imuCount = 0;

    // 遍历rosbag的每一消息
    // for (const rosbag::MessageInstance& msg : bagView) {
    //     // 在这里处理每一条消息，可以根据消息的类型进行相应的操作
    //     if (msg.getTopic() == "/velodyne_points" && msg.getDataType() == "sensor_msgs/PointCloud2") {
    //         sensor_msgs::PointCloud2::ConstPtr cloud_msg = msg.instantiate<sensor_msgs::PointCloud2>();
    //         if (cloud_msg != nullptr) {
    //             // PointCloudXYZI::Ptr pcl_cloud(new PointCloudXYZI());
    //             // pcl::fromROSMsg(*cloud_msg, *pcl_cloud);
    //             // // pcl_cloud->points[0].curvature;
    //             // ROS_INFO_STREAM("curvature: " << pcl_cloud->points[0].curvature);
    //             // ROS_INFO("Timestamp: %f", msg.getTime().toSec());
    //             lidarCount++;
    //         }
    //     } else if(msg.getTopic() == "/handsfree/imu" && msg.getDataType() == "sensor_msgs/Imu"){
    //         sensor_msgs::Imu::ConstPtr imu_msg = msg.instantiate<sensor_msgs::Imu>();
    //         if (imu_msg != nullptr) {
    //             imuCount++;
    //         }
    //     }
    //     ROS_INFO_STREAM("lidarCount : " << lidarCount << " imuCount : " << imuCount);
    // }

    float time =   23424.726498;
    float time_2 = 23424.614498;
    float time_3 = 1703815833.753305;
    std::cout << std::fixed << std::setprecision(6) << std::endl;
    std::cout << "Minimum float value: " << std::numeric_limits<float>::min()- float(0.00001) << std::endl;
    std::cout << "Maximum float value: " << std::numeric_limits<float>::max()- float(10.23411) << std::endl;
    std::cout << "2 time: " << time_3 << std::endl;    // 这里计算好奇怪啊

    // 关闭rosbag文件
    inputBag.close();

    return 0;
}

